function Xrsb = I2C_primary(S_i,u,DU,VU,t)

Xisp = [S_i(1:3)/DU;S_i(4:6)/VU];
%[] Normalize

d = -u;
Xibp = [-d*cos(t);-d*sin(t);0;d*sin(t);-d*cos(t);0];
Xisb = Xisp-Xibp;

Tir = [cos(t),sin(t),0;...
       -sin(t),cos(t),0;...
       0,0,1];
Tirdot = [-sin(t),cos(t),0;...
       -cos(t),-sin(t),0;...
       0,0,0];

Xrsb = [Tir,zeros(3);Tirdot,Tir]*Xisb;

% 1 normalize

end